VERSION 1.0 CLASS
BEGIN
  MultiUse = -1  'True
END
Attribute VB_Name = "RobotConfig"
Attribute VB_Creatable = False
Attribute VB_Exposed = False
' ************************************************
' A robot configuration.
' ************************************************
Option Explicit

Public Cx As Integer    ' Location of top of head.
Public Cy As Integer
Public LShoulderAngle As Single
Public RShoulderAngle As Single
Public LElbowAngle As Single
Public RElbowAngle As Single
Public LHipAngle As Single
Public RHipAngle As Single
Public LKneeAngle As Single
Public RKneeAngle As Single
' ************************************************
' Fill in this configuration's parameters by
' copying from another.
' ************************************************
Public Sub CopyFrame(from_me As RobotConfig)
    With from_me
        Cx = .Cx
        Cy = .Cy
        LShoulderAngle = .LShoulderAngle
        RShoulderAngle = .RShoulderAngle
        LElbowAngle = .LElbowAngle
        RElbowAngle = .RElbowAngle
        LHipAngle = .LHipAngle
        RHipAngle = .RHipAngle
        LKneeAngle = .LKneeAngle
        RKneeAngle = .RKneeAngle
    End With
End Sub


' ************************************************
' Return the position of part of the robot.
' ************************************************
Public Sub Position(part As Integer, x As Integer, y As Integer)
    Select Case part
        Case PART_HEAD
            x = Cx
            y = Cy
        Case PART_NECK
            x = Cx
            y = Cy + 2 * HEAD_RAD
        Case PART_SHOULDERS
            x = Cx
            y = Cy + 2 * HEAD_RAD + NECK_LEN
        Case PART_LELBOW
            x = Cx + _
                UARM_LEN * Cos(LShoulderAngle)
            y = Cy + 2 * HEAD_RAD + NECK_LEN - _
                UARM_LEN * Sin(LShoulderAngle)
        Case PART_RELBOW
            x = Cx + _
                UARM_LEN * Cos(RShoulderAngle)
            y = Cy + 2 * HEAD_RAD + NECK_LEN - _
                UARM_LEN * Sin(RShoulderAngle)
        Case PART_LHAND
            x = Cx + _
                UARM_LEN * Cos(LShoulderAngle) + _
                LARM_LEN * Cos(LElbowAngle)
            y = Cy + 2 * HEAD_RAD + NECK_LEN - _
                UARM_LEN * Sin(LShoulderAngle) - _
                LARM_LEN * Sin(LElbowAngle)
        Case PART_RHAND
            x = Cx + _
                UARM_LEN * Cos(RShoulderAngle) + _
                LARM_LEN * Cos(RElbowAngle)
            y = Cy + 2 * HEAD_RAD + NECK_LEN - _
                UARM_LEN * Sin(RShoulderAngle) - _
                LARM_LEN * Sin(RElbowAngle)
        Case PART_HIPS
            x = Cx
            y = Cy + 2 * HEAD_RAD + BODY_LEN
        Case PART_LKNEE
            x = Cx + _
                ULEG_LEN * Cos(LHipAngle)
            y = Cy + 2 * HEAD_RAD + BODY_LEN - _
                ULEG_LEN * Sin(LHipAngle)
        Case PART_RKNEE
            x = Cx + _
                ULEG_LEN * Cos(RHipAngle)
            y = Cy + 2 * HEAD_RAD + BODY_LEN - _
                ULEG_LEN * Sin(RHipAngle)
        Case PART_LFOOT
            x = Cx + _
                ULEG_LEN * Cos(LHipAngle) + _
                LLEG_LEN * Cos(LKneeAngle)
            y = Cy + 2 * HEAD_RAD + BODY_LEN - _
                ULEG_LEN * Sin(LHipAngle) - _
                LLEG_LEN * Sin(LKneeAngle)
        Case PART_RFOOT
            x = Cx + _
                ULEG_LEN * Cos(RHipAngle) + _
                LLEG_LEN * Cos(RKneeAngle)
            y = Cy + 2 * HEAD_RAD + BODY_LEN - _
                ULEG_LEN * Sin(RHipAngle) - _
                LLEG_LEN * Sin(RKneeAngle)
    End Select
End Sub



' ************************************************
' Draw the robot.
' ************************************************
Public Sub Draw(pic As PictureBox, handles As Boolean)
Dim x1 As Integer
Dim y1 As Integer
Dim x2 As Integer
Dim y2 As Integer
Dim x3 As Integer
Dim y3 As Integer

    ' Draw the head.
    x1 = Cx
    y1 = Cy + HEAD_RAD
    pic.Circle (x1, y1), HEAD_RAD
    If handles Then _
        pic.Line (Cx - NEAR, Cy - NEAR)- _
                Step(NEAR2, NEAR2), , BF
    
    ' Draw the body.
    y1 = y1 + HEAD_RAD
    pic.Line (x1, y1)-Step(0, BODY_LEN)
    
    ' Draw the left arm.
    y1 = y1 + NECK_LEN
    x2 = x1 + UARM_LEN * Cos(LShoulderAngle)
    y2 = y1 - UARM_LEN * Sin(LShoulderAngle)
    pic.Line (x1, y1)-(x2, y2)
    x3 = x2 + LARM_LEN * Cos(LElbowAngle)
    y3 = y2 - LARM_LEN * Sin(LElbowAngle)
    pic.Line -(x3, y3)
    If handles Then _
        pic.Line (x2 - NEAR, y2 - NEAR)- _
                Step(NEAR2, NEAR2), , BF
    If handles Then _
        pic.Line (x3 - NEAR, y3 - NEAR)- _
                Step(NEAR2, NEAR2), , BF

    ' Draw the right arm.
    x2 = x1 + UARM_LEN * Cos(RShoulderAngle)
    y2 = y1 - UARM_LEN * Sin(RShoulderAngle)
    pic.Line (x1, y1)-(x2, y2)
    x3 = x2 + LARM_LEN * Cos(RElbowAngle)
    y3 = y2 - LARM_LEN * Sin(RElbowAngle)
    pic.Line -(x3, y3)
    If handles Then _
        pic.Line (x2 - NEAR, y2 - NEAR)- _
                Step(NEAR2, NEAR2), , BF
    If handles Then _
        pic.Line (x3 - NEAR, y3 - NEAR)- _
                Step(NEAR2, NEAR2), , BF

    ' Draw the left leg.
    y1 = y1 + TRUNK_LEN
    x2 = x1 + ULEG_LEN * Cos(LHipAngle)
    y2 = y1 - ULEG_LEN * Sin(LHipAngle)
    pic.Line (x1, y1)-(x2, y2)
    x3 = x2 + LLEG_LEN * Cos(LKneeAngle)
    y3 = y2 - LLEG_LEN * Sin(LKneeAngle)
    pic.Line -(x3, y3)
    If handles Then _
        pic.Line (x2 - NEAR, y2 - NEAR)- _
                Step(NEAR2, NEAR2), , BF
    If handles Then _
        pic.Line (x3 - NEAR, y3 - NEAR)- _
                Step(NEAR2, NEAR2), , BF

    ' Draw the right leg.
    x2 = x1 + ULEG_LEN * Cos(RHipAngle)
    y2 = y1 - ULEG_LEN * Sin(RHipAngle)
    pic.Line (x1, y1)-(x2, y2)
    x3 = x2 + LLEG_LEN * Cos(RKneeAngle)
    y3 = y2 - LLEG_LEN * Sin(RKneeAngle)
    pic.Line -(x3, y3)
    If handles Then _
        pic.Line (x2 - NEAR, y2 - NEAR)- _
                Step(NEAR2, NEAR2), , BF
    If handles Then _
        pic.Line (x3 - NEAR, y3 - NEAR)- _
                Step(NEAR2, NEAR2), , BF
End Sub



' ************************************************
' Move the control point to this location.
' ************************************************
Public Sub MoveControlPoint(part As Integer, Ax As Integer, Ay As Integer, x As Integer, y As Integer)
    Select Case part
        Case PART_HEAD
            Cx = x
            Cy = y
        Case PART_LELBOW
            LShoulderAngle = Arctan2(x - Ax, Ay - y)
        Case PART_RELBOW
            RShoulderAngle = Arctan2(x - Ax, Ay - y)
        Case PART_LHAND
            LElbowAngle = Arctan2(x - Ax, Ay - y)
        Case PART_RHAND
            RElbowAngle = Arctan2(x - Ax, Ay - y)
        Case PART_LKNEE
            LHipAngle = Arctan2(x - Ax, Ay - y)
        Case PART_RKNEE
            RHipAngle = Arctan2(x - Ax, Ay - y)
        Case PART_LFOOT
            LKneeAngle = Arctan2(x - Ax, Ay - y)
        Case PART_RFOOT
            RKneeAngle = Arctan2(x - Ax, Ay - y)
    End Select
End Sub

' ************************************************
' Initialize the robot's parameters.
' ************************************************
Public Sub SetParameters(x As Integer, y As Integer, ls As Single, rs As Single, le As Single, re As Single, lh As Single, rh As Single, lk As Single, rk As Single)
    Cx = x
    Cy = y
    LShoulderAngle = ls * DEG_TO_RAD
    RShoulderAngle = rs * DEG_TO_RAD
    LElbowAngle = le * DEG_TO_RAD
    RElbowAngle = re * DEG_TO_RAD
    LHipAngle = lh * DEG_TO_RAD
    RHipAngle = rh * DEG_TO_RAD
    LKneeAngle = lk * DEG_TO_RAD
    RKneeAngle = rk * DEG_TO_RAD
End Sub

' ************************************************
' Return the distance between the top of the head
' and the top of the robot when its height is as
' large as possible.
' ************************************************
Property Get HeadRoom()
    HeadRoom = (UARM_LEN + LARM_LEN) - _
        (2 * HEAD_RAD + NECK_LEN)
End Property
' ************************************************
' Return the maximum possible height the robot
' can have.
' ************************************************
Property Get MaxHeight()
    MaxHeight = TRUNK_LEN + UARM_LEN + LARM_LEN + _
                ULEG_LEN + LLEG_LEN
End Property

' ************************************************
' Return the maximum possible width the robot
' can have.
' ************************************************
Property Get MaxWidth()
    MaxWidth = 2 * (UARM_LEN + LARM_LEN)
End Property


' ************************************************
' Read the configuration data from a file.
' ************************************************
Sub FileInput(fnum)
    Input #fnum, Cx, Cy, _
        LShoulderAngle, RShoulderAngle, _
        LElbowAngle, RElbowAngle, _
        LHipAngle, RHipAngle, _
        LKneeAngle, RKneeAngle
End Sub
' ************************************************
' Write the configuration data to a file.
' ************************************************
Sub FileWrite(fnum)
    Write #fnum, Cx, Cy, _
        LShoulderAngle, RShoulderAngle, _
        LElbowAngle, RElbowAngle, _
        LHipAngle, RHipAngle, _
        LKneeAngle, RKneeAngle
End Sub

' ************************************************
' Create a tween with fraction frac1 of the first
' configuration and (1 - frac1) of the second.
' ************************************************
Public Sub Tween(f1 As Single, config1 As RobotConfig, config2 As RobotConfig)
Dim f2 As Single

    f2 = 1# - f1
    With config1
        Cx = f1 * .Cx + f2 * config2.Cx
        Cy = f1 * .Cy + f2 * config2.Cy
        LShoulderAngle = CombinedAngle(.LShoulderAngle, config2.LShoulderAngle, f1, f2)
        RShoulderAngle = CombinedAngle(.RShoulderAngle, config2.RShoulderAngle, f1, f2)
        LElbowAngle = CombinedAngle(.LElbowAngle, config2.LElbowAngle, f1, f2)
        RElbowAngle = CombinedAngle(.RElbowAngle, config2.RElbowAngle, f1, f2)
        LHipAngle = CombinedAngle(.LHipAngle, config2.LHipAngle, f1, f2)
        RHipAngle = CombinedAngle(.RHipAngle, config2.RHipAngle, f1, f2)
        LKneeAngle = CombinedAngle(.LKneeAngle, config2.LKneeAngle, f1, f2)
        RKneeAngle = CombinedAngle(.RKneeAngle, config2.RKneeAngle, f1, f2)
    End With
End Sub
' ************************************************
' Convert the angles so they differ by no more
' than PI. Then combine them using fraction f1 of
' angle a1 and fraction f2 of a2.
' ************************************************
Function CombinedAngle(ByVal a1 As Single, ByVal a2 As Single, f1 As Single, f2 As Single) As Single
    If Abs(a1 - a2) > PI Then
        If a1 > a2 Then
            Do
                a1 = a1 - PI_TIMES_2
            Loop While a1 > a2
        Else
            Do
                a2 = a2 - PI_TIMES_2
            Loop While a2 > a1
        End If
    End If
    
    CombinedAngle = f1 * a1 + f2 * a2
End Function

